#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <helper/helper_utils.h>

using namespace std;


// 运行示例： rosrun helper helper_twist_info /cmd_vel

/**
 * @description: 
 * @return {*}
 */
class TwistInfo{
    public:
        TwistInfo();
        TwistInfo(string twist_topic);
        ~TwistInfo();


    private:
        ros::NodeHandle __nh;
        ros::Subscriber __twistSub;

        // 速度回调函数
        void TwistCallback(const geometry_msgs::TwistConstPtr& msg);

};


/**
 * @description: 
 * @return {*}
 */
TwistInfo::TwistInfo(){
    __twistSub = __nh.subscribe("/cmd_vel", 1, &TwistInfo::TwistCallback, this);
}

TwistInfo::TwistInfo(string twist_topic){
    __twistSub = __nh.subscribe(twist_topic, 1, &TwistInfo::TwistCallback, this);
}

/**
 * @description: 
 * @return {*}
 */
TwistInfo::~TwistInfo(){
    
}


/**
 * @description: 
 * @param {TwistConstPtr&} msg
 * @return {*}
 */
void TwistInfo::TwistCallback(const geometry_msgs::TwistConstPtr& msg){
    // 打印twist数据 打印线速度x,y 和角速度
    ROS_INFO("----------------------[TwistCallback]----------------------");
    ROS_INFO("[%s] linear_x: %f, linear_y: %f , angular_z: %f",
        helper::stamp_to_standard(ros::Time::now().sec).c_str(),
        msg->linear.x,
        msg->linear.y,
        msg->angular.z);
}


/**
 * @description: 
 * @param {int} argc
 * @param {char} *
 * @return {*}
 */
int main(int argc, char **argv ){

    // 打印参数
    for(int i=0; i<argc; i++){
        std::cout << "i: " << i << ", data: " << argv[i] << std::endl;
    }
    if(argc < 2){
        std::cout << "please set twist topic."  << std::endl;
        return 1;
    }
    ros::init(argc, argv, "helper_twist_info");
    ROS_INFO("helper_twist_info initialization ......");
    ros::NodeHandle nh;
    TwistInfo twist_info(argv[1]);
    ros::spin();
    return 0;
}